/*
  Copyright (c) 2010 Les Newell. All rights reserved

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program. If not, see <http://www.gnu.org/licenses/>.
*/


bool g_errFirst = true;
static const char * g_errHdr = "ERROR:";

void _CheckError()
{
    if (g_errFirst)
    {
        ERROR_PORT.print(g_errHdr);
        g_errFirst = false;
    }
}

void Error(const char * src)
{
    _CheckError();
    ERROR_PORT.print(src);
}

void Errorln(const char * src)
{
    _CheckError();
    ERROR_PORT.println(src);
    g_errFirst = true;
}

void Error(const int src)
{
    _CheckError();
    ERROR_PORT.print(src);
}

void Errorln(const int src)
{
    _CheckError();
    ERROR_PORT.println(src);
    g_errFirst = true;
}

void Error(const float src)
{
    _CheckError();
    ERROR_PORT.print(src);
}

void Errorln(const float src)
{
    _CheckError();
    ERROR_PORT.println(src);
    g_errFirst = true;
}




#define MAX_CMD_LENGTH 50

#define EOT '\n'

DEFINE_MODULE(SerialComms,Module)


public:
    SerialComms(HardwareSerial& port, long int baud)
    {
//        AddSocket("SerialComms.UpdateRate",&m_updateRate);
        m_port = &port;
        m_baud = baud;
    }

    //perform your initialization here
    void Init()
    {
        m_port->begin(m_baud);
        SetInterval(20);
        SetPriority(0);
        m_bufIdx = 0;
//        m_updateRate = 100;
        m_port->println("#######################");
        m_port->println("#                     #");
        m_port->println("# Welcome to FlightOS #");
        m_port->println("#                     #");
        m_port->println("#######################");
    }

    //This function is called at the rate specified by SetInterval
    virtual void Loop(const unsigned long& interval)
    {
        while (m_port->available() >0)
        {
            char c = m_port->read();
            if (c == EOT)
            {
                m_cmdBuffer[m_bufIdx] = 0;
                HandleCommand();
                return;
            }
            if (c < ' ')
            {
                continue;
            }
            if (m_bufIdx >= MAX_CMD_LENGTH -1)
            {
                m_bufIdx = 0;
                Errorln("Buffer overflow");
            }
            m_cmdBuffer[m_bufIdx++] = c;
        }

        static int loopCount = 1;

/*        if (m_run)
        {
            loopCount--;
            if (loopCount ==0)
            {
                loopCount = m_updateRate/10;
                m_port->print("*");
                for (int ct=0; ct< m_list.GetCount(); ct++)
                {
                    if (ct !=0 )
                    {
                        m_port->print(", ");
                    }
                    m_list[ct]->Print(MAIN_COMMS);
                }
                m_port->println("");
            }
        }*/

    }

private:
    void HandleCommand()
    {
        m_bufIdx = 0;
        bool stop = true;
        bool err = false;
        switch (m_cmdBuffer[0])
        {
        case 'g': //get socket/parameter
            PrintSocket();
            break;

        case 's': //set parameter
            SetParameter();
            break;

        case 'l': //list modules
            ListModules();
            break;

        case 'a': //append list
            AppendList();
            break;

        case 'c': //clear list
            m_list.Empty();
            break;

        case 'r': //run list
            m_port->print("*");
            for (int ct=0; ct< m_list.GetCount(); ct++)
            {
                if (ct !=0 )
                {
                    m_port->print(", ");
                }
                m_list[ct]->Print(*m_port);
            }
            m_port->println("");
            err = true;

//            m_run = true;
//            stop = false;
            break;

        case 'w': //write to EEPROM
            if (strcmp(m_cmdBuffer,"writeEEPROM") !=0)
            {
                Errorln("Invalid memory write command");
            }
            else
            {
                g_moduleManager.WriteParameters();
            }
            break;

        case 'p': //ping
            break;

        case 'd': //diagnostics
            ShowDiagnostics();
            break;

        case 'v': //version
            g_moduleManager.PrintVersion(*m_port,true);
            m_port->println();
            break;

        default:
            err = true;
            Errorln("Unknown command");
        }
        if (!err)
        {
            m_port->println("ok");
        }
        if (stop)
        {
            m_run = false;
        }
    }

    void PrintSocket()
    {
        Socket * val = g_moduleManager.GetSocket(m_cmdBuffer + 1);
        if (!val)
        {
            return;
        }
        val->Print(*m_port);
        m_port->println("");
    }

    void SetParameter()
    {
        char * parPtr = m_cmdBuffer + 1;
        while (*parPtr !=0 && *parPtr != '=')
        {
            parPtr++;
        }
        if (*parPtr == 0)
        {
            Errorln("No number");
            return;
        }
        *parPtr=0;
        parPtr++;

        g_moduleManager.ParseParameter(m_cmdBuffer + 1,parPtr);
    }

    void ListModules()
    {
        for (int ct=0; ct< g_moduleManager.GetCount(); ct++)
        {
            Module * mod = g_moduleManager.GetModule(ct);
            mod->PrintName(*m_port);
            m_port->println();
            SocketArray& sockets = mod->GetSockets();
            for (int ct2=0; ct2 < sockets.GetCount(); ct2++)
            {
                SOCKETRECORD& rec = sockets[ct2];
                m_port->print("  ");
#ifdef ARDUINO
                PrintProgString(m_port,rec.name);
#else
                m_port->print(rec.name);
#endif
                if (rec.socket->IsParameter())
                {
                    m_port->print(" W ");
                }
                else
                {
                    m_port->print(" R ");
                }
                m_port->print(rec.socket->GetType());
                m_port->print(" ");
                m_port->println((ct << 8) + ct2,HEX);
            }
        }
    }


    void ShowDiagnostics()
    {
        //Method to get the free memory on the arduino
        //http://www.arduino.cc/playground/Code/AvailableMemory
        uint8_t *heapptr, *stackptr, *testptr;
        stackptr = (uint8_t *)malloc(4);          // use stackptr temporarily
        heapptr = stackptr;                     // save socket of heap pointer
        free(stackptr);      // free up the memory again (sets stackptr to 0)
        stackptr =  (uint8_t *)(SP);           // save socket of stack pointer

        m_port->print("Heap ptr = ");
        m_port->println((long int)heapptr);
        m_port->print("Stack ptr = ");
        m_port->println((long int)stackptr);


        //Scan for a uninterrupted series of zeros - this is the memory
        //that has never been used
        int ct=0;
        testptr = heapptr;
        while(testptr < stackptr)
        {
            if(*testptr++ !=0)
            {
                if(ct < 20)
                {
                    ct=0;
                    continue;
                }else
                {
                    break;
                }
            }
            ct++;
        }

        m_port->print("CPU load = ");
        m_port->print(GetCPULoad());
        m_port->println("%");
        m_port->print("Max number of threads = ");
        m_port->println(g_moduleManager.GetMaxThreads());
        m_port->print("Ran out of threads ");
        m_port->print(g_moduleManager.GetOverloadCount());
        m_port->println(" times");
        m_port->print("Heartbeat = ");
        m_port->println(g_moduleManager.GetHeartbeat());
        m_port->print("Current free memory = ");
        m_port->println(stackptr - heapptr);
        m_port->print("Minimum free memory = ");
        m_port->println(ct);
        for (int ct=0; ct< g_moduleManager.GetCount(); ct++)
        {
            Module * mod = g_moduleManager.GetModule(ct);
            m_port->print(mod->GetName());
            m_port->print(": Priority = ");
            m_port->print((int)mod->GetPriority());
            m_port->print(": Interval = ");
            m_port->print((int)mod->GetInterval());
            m_port->print(" Max execution time = ");
            m_port->println(mod->GetMaxTime());
        }
        m_port->println();

    }

    void AppendList()
    {
        Socket * val = g_moduleManager.GetSocket(m_cmdBuffer + 1);
        if (!val)
        {
            return;
        }
        m_list.Add(val);
    }


private:
    char m_cmdBuffer[MAX_CMD_LENGTH];
    int m_bufIdx;
    bool m_run;
    GrowArray<Socket *> m_list;
//    Parameter8 m_updateRate;
    HardwareSerial * m_port;
    long int m_baud;

};

